/**********************************************
 *                                            *
 * bill the robot                             *
 * v 0.4                                      *
 * Jack Murray                                *
 *                                            *
 **********************************************/

// Libraries
#include <Servo.h>

// Setup the servo
Servo pingServo;

// Setup Pins
const int pingPin = 7;
const int directionA = 12;
const int directionB = 13;
const int speedA = 3;
const int speedB = 11;
const int brakeA = 9;
const int brakeB = 8;
const int greenLed = 2;
const int redLed = 3;

// setup variables
int pingDistance;
int pingDelay = 100;
int pingLeft;
int pingRight;
int pingForward;

int time;

void setup()
{
  //attach servos
  pingServo.attach(5);
  
  //setup LEDs
  pinMode(greenLed, OUTPUT);
  pinMode(redLed, OUTPUT);
  digitalWrite(greenLed, HIGH);
  
  //setup motorshield
  pinMode(directionA, OUTPUT);
  pinMode(directionB, OUTPUT);
  pinMode(speedA, OUTPUT);
  pinMode(speedB, OUTPUT);
  pinMode(brakeA, OUTPUT);
  pinMode(brakeB, OUTPUT);
  
  Serial.begin(9600);
  
  // set initial servo positions
  pingServo.write(100);
  
  //delay after power applied
  delay(5000);
}

void loop()
{
  directionalPing();
  if(pingLeft <= 6)
    {
      reverse(50);
      turnRight(600);
      return;
    }
  if(pingRight <= 6)
    {
      reverse(50);
      turnLeft(600);
      return;
    }
  if(pingForward <= 6)
    {
      reverse(200);
      turnLeft(600);
      return;
    }
  if(pingForward > 6)
    {
      reverse(50);
      forward(150);
      return;
    }
}

long turnLeft(long time)
{
  digitalWrite(redLed, HIGH);
  Serial.println("turning left");
  
  digitalWrite(directionA, LOW);
  digitalWrite(directionB, LOW);
  
  digitalWrite(brakeB, LOW);
  digitalWrite(brakeA, LOW);
  
  digitalWrite(speedA, HIGH);
  digitalWrite(speedB, HIGH);
  delay(time);
  
  digitalWrite(redLed, LOW);
}

long turnRight(long time)
{
  digitalWrite(redLed, HIGH);
  Serial.println("turning right");
  
  digitalWrite(directionA, HIGH);
  digitalWrite(directionB, HIGH);
  
  digitalWrite(brakeB, LOW);
  digitalWrite(brakeA, LOW);
  
  digitalWrite(speedA, HIGH);
  digitalWrite(speedB, HIGH);
  delay(time);
  
  digitalWrite(redLed, LOW);
}

long forward(long time)
{
  digitalWrite(redLed, HIGH);
  Serial.println("driving forward");
  
  digitalWrite(directionA, LOW);
  digitalWrite(directionB, HIGH);
  
  digitalWrite(brakeB, LOW);
  digitalWrite(brakeA, LOW);
  
  digitalWrite(speedA, HIGH);
  digitalWrite(speedB, HIGH);
  delay(time);
  
  digitalWrite(redLed, LOW);
}

long reverse(long time)
{
  digitalWrite(redLed, HIGH);
  Serial.println("driving backwards");
  
  digitalWrite(directionA, HIGH);
  digitalWrite(directionB, LOW);
  
  digitalWrite(brakeB, LOW);
  digitalWrite(brakeA, LOW);
  
  digitalWrite(speedA, HIGH);
  digitalWrite(speedB, HIGH);
  delay(time);
  
  digitalWrite(redLed, LOW);
}

long brake(long time)
{
  digitalWrite(speedA, LOW);
  digitalWrite(speedB, LOW);
  digitalWrite(brakeB, HIGH);
  digitalWrite(brakeA, HIGH);
  delay(time);
}
void directionalPing()
{
  pingServo.write(150);
  delay(500);
  ping();
  pingLeft = pingDistance;
  pingServo.write(100);
  delay(500);
  ping();
  pingForward = pingDistance;
  pingServo.write(50);
  delay(500);
  ping();
  pingRight = pingDistance;
  
  Serial.print("left ");
  Serial.println(pingLeft);
  Serial.print("forward ");
  Serial.println(pingForward);
  Serial.print("right ");
  Serial.println(pingRight);
  Serial.println();
}
  
void ping()
{
  long duration, inches;
  
  //trigger the ping)) sensor
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);
  
  //read the ping)) sensor
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);
  
  // convert the time into a distance
  inches = microsecondsToInches(duration);
  
  //output PingDistance
  pingDistance = inches;
}
  
long microsecondsToInches(long microseconds)
{
  return microseconds / 74 / 2;
}
  

